#include <QtGui>
#include <QMessageBox>
#include <iostream>
#include "../include/ros_robot/main_window.hpp"

namespace ros_robot {

using namespace Qt;

    MainWindow::MainWindow(int argc, char** argv, QWidget *parent): QMainWindow(parent), qnode(argc,argv){
        ui.setupUi(this); // Calling this incidentally connects all ui's triggers to on_...() callbacks in this class.
        QObject::connect(ui.actionAbout_Qt, SIGNAL(triggered(bool)), qApp, SLOT(aboutQt())); // qApp是应用程序的全局变量

        ReadSettings();
        setWindowIcon(QIcon(":/images/icon.png"));

        QObject::connect(&qnode, SIGNAL(rosShutdown()), this, SLOT(close()));

        if ( ui.checkbox_remember_settings->isChecked() ) {
            on_button_connect_clicked(true);
        }
        
        
        
        connect(ui.horizontalSlider_linear,SIGNAL(valueChanged(int)),this,SLOT(slot_linear_value_changed(int)));
        connect(ui.horizontalSlider_angular,SIGNAL(valueChanged(int)),this,SLOT(slot_angular_value_changed(int)));
        connect(ui.pushButton_1,SIGNAL(clicked()),this,SLOT(slot_pushButton_click()));
        connect(ui.pushButton_2,SIGNAL(clicked()),this,SLOT(slot_pushButton_click()));
        connect(ui.pushButton_3,SIGNAL(clicked()),this,SLOT(slot_pushButton_click()));
        connect(ui.pushButton_4,SIGNAL(clicked()),this,SLOT(slot_pushButton_click()));
        connect(ui.pushButton_6,SIGNAL(clicked()),this,SLOT(slot_pushButton_click()));
        connect(ui.pushButton_7,SIGNAL(clicked()),this,SLOT(slot_pushButton_click()));
        connect(ui.pushButton_8,SIGNAL(clicked()),this,SLOT(slot_pushButton_click()));
        connect(ui.pushButton_9,SIGNAL(clicked()),this,SLOT(slot_pushButton_click()));

        speed_x_dashBoard=new CCtrlDashBoard(ui.widget_speed_x);
        speed_y_dashBoard=new CCtrlDashBoard(ui.widget_speed_y);
        speed_x_dashBoard->setGeometry(ui.widget_speed_x->rect());
        speed_y_dashBoard->setGeometry(ui.widget_speed_y->rect());
        speed_x_dashBoard->setValue(0);
        speed_y_dashBoard->setValue(0);

        connect(&qnode,SIGNAL(speed_vel(float,float)),this,SLOT(slot_update_dashBoard(float,float)));
        
        connect(&qnode,SIGNAL(power_vel(float)),this,SLOT(slot_update_power(float)));

        connect(&qnode,SIGNAL(image_val(QImage)),this,SLOT(slot_update_image(QImage)));
        connect(ui.pushButton_sub_image,SIGNAL(clicked()),this,SLOT(slot_sub_image()));
        
        cmd = new QProcess;
        connect(ui.pushButton_start_turtlebot3,SIGNAL(clicked()),this,SLOT(slot_pushButton_start_turtlebot3_clicked()));
        connect(ui.pushButton_shutdown_turtlebot3,SIGNAL(clicked()),this,SLOT(slot_pushButton_shutdowm_turtlebot3_clicked()));
        connect(ui.pushButton_cmd,SIGNAL(clicked()),this,SLOT(slot_pushButton_cmd_clicked()));
    }

    MainWindow::~MainWindow() {}
    
    void MainWindow::showNoMasterMessage() {
        QMessageBox msgBox;
        msgBox.setText("can't find ros master. ");
        msgBox.exec();
        close();
    }

    void MainWindow::on_button_connect_clicked(bool check ) {
        if ( ui.checkbox_use_environment->isChecked() ) {
            if ( !qnode.init() ) {
                showNoMasterMessage();
            }
            else {
                ui.button_connect->setEnabled(false);
            }
        }
        else {
            if ( ! qnode.init(ui.line_edit_master->text().toStdString(),ui.line_edit_host->text().toStdString()) ) {
                showNoMasterMessage();
            }
            else {
                ui.button_connect->setEnabled(false);
                ui.line_edit_master->setReadOnly(true);
                ui.line_edit_host->setReadOnly(true);
                ui.line_edit_topic->setReadOnly(true);
            }
        }
    }

    void MainWindow::on_checkbox_use_environment_stateChanged(int state) {
        bool enabled;
        if ( state == 0 ) {
            enabled = true;
        }
        else {
            enabled = false;
        }
        ui.line_edit_master->setEnabled(enabled);
        ui.line_edit_host->setEnabled(enabled);
    }

    void MainWindow::on_actionAbout_triggered() {
        QMessageBox::about(this, tr("About ..."),tr("<h2>PACKAGE_NAME Test Program 0.10</h2><p>Copyright Yujin Robot</p><p>This package needs an about description.</p>"));
    }

    void MainWindow::ReadSettings() {
        QSettings settings("Qt-Ros Package", "ros_robot");
        restoreGeometry(settings.value("geometry").toByteArray());
        restoreState(settings.value("windowState").toByteArray());
        QString master_url = settings.value("master_url",QString("http://192.168.1.2:11311/")).toString();
        QString host_url = settings.value("host_url", QString("192.168.1.3")).toString();

        ui.line_edit_master->setText(master_url);
        ui.line_edit_host->setText(host_url);

        bool remember = settings.value("remember_settings", false).toBool();
        ui.checkbox_remember_settings->setChecked(remember);
        bool checked = settings.value("use_environment_variables", false).toBool();
        ui.checkbox_use_environment->setChecked(checked);
        if ( checked ) {
            ui.line_edit_master->setEnabled(false);
            ui.line_edit_host->setEnabled(false);
        }
    }

    void MainWindow::WriteSettings() {
        QSettings settings("Qt-Ros Package", "ros_robot");
        settings.setValue("master_url",ui.line_edit_master->text());
        settings.setValue("host_url",ui.line_edit_host->text());

        settings.setValue("use_environment_variables",QVariant(ui.checkbox_use_environment->isChecked()));
        settings.setValue("geometry", saveGeometry());
        settings.setValue("windowState", saveState());
        settings.setValue("remember_settings",QVariant(ui.checkbox_remember_settings->isChecked()));
    }

    void MainWindow::closeEvent(QCloseEvent *event){
        WriteSettings();
        QMainWindow::closeEvent(event);
    }




    void MainWindow::slot_linear_value_changed(int value){//add
      ui.label_linear_number->setText(QString::number(value));
    }

    void MainWindow::slot_angular_value_changed(int value){//add
      ui.label_angular_number->setText(QString::number(value));
    }

    void MainWindow::slot_pushButton_click(){//add
      QPushButton* pb=qobject_cast<QPushButton*> (sender());
      qDebug()<<pb->text();
      char k=pb->text().toStdString()[0];
      bool is_all=ui.checkBox_is_all->isChecked();
      double linear=ui.label_linear_number->text().toDouble()*0.01;
      double angular=ui.label_angular_number->text().toDouble()*0.01;
      //double linear=ui.horizontalSlider_linear->value()*0.01;
      //double angular=ui.horizontalSlider_angular->value()*0.01;

      //std::cout<<linear<<std::endl;
      //std::cout<<angular<<std::endl;

      switch(k){
      case '7':
        qnode.set_cmd_vel(is_all?'U':'u',linear,angular);
        break;
      case '8':
        qnode.set_cmd_vel(is_all?'I':'i',linear,angular);
        break;
      case '9':
        qnode.set_cmd_vel(is_all?'O':'o',linear,angular);
        break;
      case '4':
        qnode.set_cmd_vel(is_all?'J':'j',linear,angular);
        break;
      case '6':
        qnode.set_cmd_vel(is_all?'L':'l',linear,angular);
        break;
      case '1':
        qnode.set_cmd_vel(is_all?'M':'m',linear,angular);
        break;
      case '2':
        qnode.set_cmd_vel(is_all?'<':',',linear,angular);
        break;
      case '3':
        qnode.set_cmd_vel(is_all?'>':'.',linear,angular);
        break;
      }
    }

    void MainWindow::slot_update_dashBoard(float x,float y){//add
      ui.label_dir_x->setText(x>=0?"D":"R");
      ui.label_dir_y->setText(y>=0?"D":"R");
      speed_x_dashBoard->setValue(std::abs(x)*100);
      speed_y_dashBoard->setValue(std::abs(y)*100);
    }

    void MainWindow::slot_update_power(float value){
        ui.label_power_value->setText(QString::number(value).mid(0.5)+"V");
        ui.progressBar->setValue(value);
    }

    void MainWindow::slot_update_image(QImage im){//add
        ui.label_image->setPixmap(QPixmap::fromImage(im));
        std::cout<<"update sucess"<<std::endl;
    }

    void MainWindow::slot_sub_image(){//add

        qnode.sub_image(ui.lineEdit_image_topic->text());;
        std::cout<<"click sucess"<<std::endl;
    }

    void MainWindow::slot_pushButton_start_turtlebot3_clicked(){//add
        
        cmd->start("bash");
        cmd->write("export TURTLEBOT3_MODEL=burger\n");//必须在这个终端设置TURTLEBOT3_MODEL环境变量
        cmd->write("roslaunch turtlebot3_fake turtlebot3_fake.launch\n");
                     
        connect(cmd,SIGNAL(readyReadStandardOutput()),this,SLOT(slot_textEdit_output()));
        connect(cmd,SIGNAL(readyReadStandardError()),this,SLOT(slot_textEdit_output()));
    }
    
    void MainWindow::slot_pushButton_shutdowm_turtlebot3_clicked(){//add
        cmd->start("bash");
        cmd->write("killall roslaunch\n");
        sleep(2);
    }
    
    void MainWindow::slot_pushButton_cmd_clicked(){//add
        
        cmd = new QProcess;
        cmd->start("bash");
        cmd->write(ui.lineEdit_input->text().toLocal8Bit()+"\n");
                
        connect(cmd,SIGNAL(readyReadStandardOutput()),this,SLOT(slot_textEdit_output()));
        connect(cmd,SIGNAL(readyReadStandardError()),this,SLOT(slot_textEdit_output()));
        
        /*注意拼写，一个是准备读取标准输出readyReadStandardOutput()，它是有新的就读取;
         * 一个是读取所有的标准输出readAllStandardOutput(),它要等待结束waitForFinished()才可以读取*/          
    }

    void MainWindow::slot_textEdit_output(){
        //cmd->waitForFinished();
        QString result_error = cmd->readAllStandardError();
        qDebug() << result_error;
        QString result_output = cmd->readAllStandardOutput();
        qDebug() << result_output;
        
        ui.textEdit_output->append("<font color=\"#FF0000\">" + result_error + "</font>");
        ui.textEdit_output->append("<font color=\"#000000\">" + result_output + "</font>");
    }
}

